#!/usr/bin/env python
PACKAGE = "ipa_room_exploration"

from dynamic_reconfigure.parameter_generator_catkin import *

gen = ParameterGenerator()

# Method to plan a path trough the room
method_enum = gen.enum([ gen.const("GridPointExplorator", int_t, 1, "Use the grid point exploration algorithm."),
			gen.const("BoustrophedonExplorator", int_t, 2, "Use the boustrophedon exploration algorithm."),
			gen.const("NeuralNetworkExplorator", int_t, 3, "Use the neural network exploration algorithm."),
			gen.const("ConvexSPPExplorator", int_t, 4, "Use the convexSPP exploration algorithm."),
			gen.const("FlowNetworkExplorator", int_t, 5, "Use the flowNetwork exploration algorithm."),
			gen.const("EnergyFunctionalExplorator", int_t, 6, "Use the energyFunctional exploration algorithm."),
			gen.const("VoronoiExplorator", int_t, 7, "Use the voronoi exploration algorithm."),
			gen.const("BoustrophedonVariantExplorator", int_t, 8, "Use a variant of the boustrophedon exploration algorithm.")],
			"Exploration algorithm")
gen.add("room_exploration_algorithm", int_t, 0, "Exploration method", 2, 1, 8, edit_method=method_enum)


# Parameters on map correction options
# ====================================
gen.add("map_correction_closing_neighborhood_size", int_t, 0, "Applies a closing operation to neglect inaccessible areas and map errors/artifacts if the map_correction_closing_neighborhood_size parameter is larger than 0. The parameter then specifies the iterations (or neighborhood size) of that closing operation..", 2, -1, 100);


# Parameters specific to the navigation of the robot along the computed coverage trajectory
# =========================================================================================
gen.add("return_path", bool_t, 0, "Boolean used to determine whether the server should return the computed coverage path in the response message.", True)

gen.add("execute_path", bool_t, 0, "Boolean used to determine whether the server should navigate the robot along the computed coverage path.", False)

# double that shows how close the robot can get to a published navigation goal before another gets published
gen.add("goal_eps", double_t, 0, "Distance from robot to published goal to publish the next goal in the path.", 2.0, 1.0)

# Use a dynamic goal distance criterion: the larger the path's curvature, the more accurate the navigation.
# Uses 'goal_eps' param as maximum distance on straight paths and zero distance (accurate goal approaching) on 90deg curves.
gen.add("use_dyn_goal_eps", bool_t, 0, "Use a dynamic goal distance criterion: the larger the path's curvature, the more accurate the navigation.", False)

# Boolean that interrupts the publishing of the navigation goals as long as needed, e.g. when the robot sees a trashbin during a cleaning task and wants to clean it directly and resume later with the coverage path.
gen.add("interrupt_navigation_publishing", bool_t, 0, "Interrupt the publishing of navigation goals as long as needed.", False)

# turn the functionality on/off to revisit areas that haven't been seen during the execution of the coverage path, due to uncertainties or dynamic obstacles
gen.add("revisit_areas", bool_t, 0, "Revisiting not seen areas due to dynamic obstacles on/off.", True)

# min area before previously not seen areas have to be revisited, [m^2]
gen.add("left_sections_min_area", double_t, 0, "Minimal size of left sections to revisit them after one go [m^2].", 0.01, 1e-7)

gen.add("global_costmap_topic", str_t, 0, "The name of the global costmap topic.", "/move_base/global_costmap/costmap")

gen.add("coverage_check_service_name", str_t, 0, "The name of the service to call for a coverage check of the driven trajectory.", "/room_exploration/coverage_check_server/coverage_check")

gen.add("map_frame", str_t, 0, "The name of the map frame, used for tracking of the robot.", "map")

gen.add("camera_frame", str_t, 0, "The name of the camera frame, that is in the same kinematic chain as the map_frame and shows the camera pose.", "base_link")


# Grid point Explorator
# =====================
tsp_solver_enum = gen.enum([ gen.const("NearestNeighborTSP", int_t, 1, "Use the Nearest Neighbor TSP algorithm."),
			gen.const("GeneticTSP", int_t, 2, "Use the Genetic TSP solver."),
			gen.const("ConcordeTSP", int_t, 3, "Use the Concorde TSP solver.")],
			"Indicates which TSP solver should be used.")
gen.add("tsp_solver", int_t, 0, "Exploration method", 3, 1, 3, edit_method=tsp_solver_enum)

gen.add("tsp_solver_timeout", int_t, 0, "A sophisticated solver like Concorde or Genetic can be interrupted if it does not find a solution within this time (in [s]), and then falls back to the nearest neighbor solver.", 600, 1);


# Boustrophedon Explorator
# ========================
gen.add("min_cell_area", double_t, 0, "Minimum area of one cell for the boustrophedon explorator.", 10.0, 1e-7)

# min distance between two points in the created coverage path
gen.add("path_eps", double_t, 0, "Minimal distance between two points on the generated path [pixel].", 2.0, 0)

# the additional offset of the grid to obstacles, i.e. allows to displace the grid by more than the standard half_grid_size from obstacles, in [m]
gen.add("grid_obstacle_offset", double_t, 0, "Allows to displace the grid by more than the standard half_grid_size from obstacles [m].", 0.0, 0)

# maximal allowed shift off the ideal boustrophedon track to both sides for avoiding obstacles on track
# setting max_deviation_from_track=grid_spacing is usually a good choice
# for negative values (e.g. max_deviation_from_track: -1) max_deviation_from_track is automatically set to grid_spacing
# in [pixel]
gen.add("max_deviation_from_track", int_t, 0, "Maximal allowed shift off the ideal boustrophedon track for avoiding obstacles on track, in [pixel]. For negative values max_deviation_from_track is automatically set to grid_spacing.", -1, -1)

# enum for cell visiting order
cell_visiting_order_enum = gen.enum([gen.const("OptimalTSP", int_t, 1, "The optimal visiting order of the cells is determined as TSP problem."),
			gen.const("LeftToRight", int_t, 2, "Alternative ordering from left to right (measured on y-coordinates of the cells), visits the cells in a more obvious fashion to the human observer (though it is not optimal).")],
			"Cell visiting order")
gen.add("cell_visiting_order", int_t, 0, "Cell visiting order method", 1, 1, 2, edit_method=cell_visiting_order_enum)


# Neural network explorator, see room_exploration_action_server.params.yaml for further details
# =============================================================================================
gen.add("step_size", double_t, 0, "Step size for integrating the state dynamics of the neural network.", 0.008, 0.0001, 1.0)

gen.add("A", int_t, 0, "Decaying parameter that pulls the activity of a neuron closer to zero, larger value means faster decreasing.", 17, 1)

gen.add("B", int_t, 0, "Increasing parameter that tries to increase the activity of a neuron, higher value means a higher desired value and a faster increasing at the beginning.", 5, 1)

gen.add("D", int_t, 0, "Decreasing parameter when the neuron is labeled as obstacle, higher value means faster decreasing.", 7, 1)

gen.add("E", int_t, 0, "External input parameter of one neuron that is used in the dynamics corresponding to if it is an obstacle or uncleaned/cleaned, E>>B.", 80, 1)

gen.add("mu", double_t, 0, "Parameter to set the importance of the states of neighboring neurons to the dynamics, higher value means higher influence.", 1.03, 0.001)

gen.add("delta_theta_weight", double_t, 0, "Parameter to set the importance of the traveleing direction from the previous step and the next step, a higher value means that the robot should turn less.", 0.15, 0.0)


# ConvexSPP explorator
# ====================
# size of one cell when discretizing the free space
gen.add("cell_size", int_t, 0, "Side length of one grid cell, when discretizing the free space.", 0, -1)

gen.add("delta_theta", double_t, 0, "Sampling angle when creating possible sensing poses.", 1.570796, 1e-4)


# flowNetwork explorator
# ======================
gen.add("curvature_factor", double_t, 0, "Factor an arc can be longer than a straight arc.", 1.1, 1.0)

gen.add("max_distance_factor", double_t, 0, "#Factor, an arc can be longer than the maximal distance of the room.", 1.0, 1.0)


exit(gen.generate(PACKAGE, "ipa_room_exploration_action_server", "RoomExploration"))
